#ifndef __UTILITY_FUNCTIONS__
#define __UTILITY_FUNCTIONS__

#include <iostream>

template <int d, typename position, typename index, typename element, 
  typename ordering, typename policy>
void
sift_down(position a, index i, element x, const ordering& less, policy& p) {
  //std::cout << " - before sift_down i,x: " << i << " " << x << std::endl;
  //show(a, a + p.size());
  //show(p.B.begin(), p.B.end());

  index j;
  index m = 63 * (p.last_leaf() / 4032);
  while (i < m) {
    index k = i;
    position ak = a + i;
    i = (less(*(ak + 1), *(ak + 2))) ? 2 : 1;
    *(ak) = *(ak + i);
    j = i;
    i = (i << 1) + 1;
    if(less(*(ak + i), *(ak + i + 1))) ++i;    
    *(ak + j) = *(ak + i);
    j = i;
    i = (i << 1) + 1;
    if(less(*(ak + i), *(ak + i + 1))) ++i;    
    *(ak + j) = *(ak + i);
    j = i;
    i = (i << 1) + 1;
    if(less(*(ak + i), *(ak + i + 1))) ++i;    
    *(ak + j) = *(ak + i);
    j = i;
    i = (i << 1) + 1;
    if(less(*(ak + i), *(ak + i + 1))) ++i;    
    *(ak + j) = *(ak + i);
    j = i;
    i += k;
    i = (126 * i) - 3843 - (3906 * (k / 63));
    if(less(*(a + i), *(a + i + 63))) i += 63;  
    if(less(*(a + i), x)) {
      if(less(*(ak + j), x)) {
	if(less(*(ak + ((j - 3) >> 2)), x)) {
	  *(ak + ((j - 1) >> 1)) = *(ak + ((j - 3) >> 2)); 
	  if(less(*(ak + ((j - 7) >> 3)), x)) {
	    *(ak + ((j - 3) >> 2)) = *(ak + ((j - 7) >> 3));	    
	    if(less(*(ak + ((j - 15) >> 4)), x)) {
	      *(ak + ((j - 7) >> 3)) = *(ak + ((j - 15) >> 4));	    
	      if(less(*(ak), x)) {
		*(ak + ((j - 15) >> 4)) =  *(ak);
		*(ak) = x;
	      }
	      else
		*(ak + ((j - 15) >> 4)) =  x;     
	    }
	    else
	      *(ak + ((j - 7) >> 3)) = x;	
	  }
	  else
	    *(ak + ((j - 3) >> 2)) = x;	  
	}
	else
	  *(ak + ((j - 1) >> 1)) = x;
      }
      else
	*(ak + j) =  x;
      return;
    }
    else
      *(ak + j) =  *(a + i);
  }
  if ((((4032 * (i / 63)) + 63) > p.last_leaf()) && ((i + 62) <= p.last_leaf())) {
    position ak = a + i;
    i = (less(*(ak + 1), *(ak + 2))) ? 2 : 1;
    if(less(*(ak + i), x)) {
      *(ak) = x;
      return;
    }
    *(ak) = *(ak + i);
    j = i;
    i = (i << 1) + 1;
    if(less(*(ak + i), *(ak + i + 1))) ++i;
    if(less(*(ak + i), x)) {
      *(ak + j) = x;
      return;
    }     
    *(ak + j) = *(ak + i);
    j = i;
    i = (i << 1) + 1;
    if(less(*(ak + i), *(ak + i + 1))) ++i;    
    if(less(*(ak + i), x)) {
      *(ak + j) = x;
      return;
    }  
    *(ak + j) = *(ak + i);
    j = i;
    i = (i << 1) + 1;
    if(less(*(ak + i), *(ak + i + 1))) ++i;    
    if(less(*(ak + i), x)) {
      *(ak + j) = x;
      return;
    } 
    *(ak + j) = *(ak + i);
    j = i;
    i = (i << 1) + 1;
    if(less(*(ak + i), *(ak + i + 1))) ++i;    
    if(less(*(ak + i), x)) {
      *(ak + j) = x;
      return;
    }
    *(ak + j) = *(ak + i);
    *(ak + i) = x;
    return;
  }
  while (p.last_child(i) <= p.last_leaf()) {
    j = p.top_all_present(a, i, less);
    if (less(x, *(a + j))) {
      p.update_all_present(a, i, *(a + j), less);
    }
    else {
      p.update_all_present(a, i, x, less);
      return;
    }
    i = j;
  }
  while(p.first_child(i) <= p.last_leaf()) {
    if(p.last_child(i) <= p.last_leaf())
      j = p.top_all_present(a, i, less);	
    else 
      j = p.top_some_absent(a, i, less);
    if (less(x, *(a + j))) {
      p.update_all_present(a, i, *(a + j), less);
      i = j;
    }
    else {
      p.update_all_present(a, i, x, less);
      return;
    }
  }    
  p.update_all_present(a, i, x, less);
  return;      

  //std::cout << "- after sift_down: " << std::endl;
  //show(a, a + p.size());
  //show(p.B.begin(), p.B.end()); 

}

template <int d, typename position, typename index, typename element, 
  typename ordering, typename policy>
void
sift_down2(position a, index i, element x, const ordering& less, policy& p) {
  //std::cout << " - before sift_down i,x: " << i << " " << x << std::endl;
  //show(a, a + p.size());
  //show(p.B.begin(), p.B.end());
  
  index j;
  while (p.last_child(i) <= p.last_leaf()) {
    j = p.top_all_present(a, i, less);
    if (less(x, *(a + j))) {
      p.update_all_present(a, i, *(a + j), less);
    }
    else {
      p.update_all_present(a, i, x, less);
      return;
    }
    i = j;
  }
  
  while(p.first_child(i) <= p.last_leaf()) {
    if(p.last_child(i) <= p.last_leaf())
      j = p.top_all_present(a, i, less);	
    else 
      j = p.top_some_absent(a, i, less);
    if (less(x, *(a + j))) {
      p.update_all_present(a, i, *(a + j), less);
      i = j;
    }
    else {
      p.update_all_present(a, i, x, less);
      return;
    }
  }    
  p.update_all_present(a, i, x, less);
  return;  

  //std::cout << "- after sift_down: " << std::endl;
  //show(a, a + p.size());
  //show(p.B.begin(), p.B.end()); 

} 

#endif // __UTILITY_FUNCTIONS__


